package com.crunch.math;

import java.util.Arrays;

/**
 * A class for 2x2 matrices.
 */
public final class Matrix22f {
	float[] d;

	/**
	 * Constructs an identity matrix.
	 */
	public Matrix22f() {
		d = new float[] { 1.0f, 0.0f, 0.0f, 1.0f };
	}

	/**
	 * Sets this matrix to the identity matrix.
	 */
	public void setIdentity() {
		d[0] = 1.0f;
		d[1] = 0.0f;
		d[2] = 0.0f;
		d[3] = 1.0f;
	}

	/**
	 * Constructs a matrix by copying the one provided.
	 */
	public Matrix22f(Matrix22f m) {
		d = Arrays.copyOf(m.d, m.d.length);
	}

	/**
	 * Sets this matrix by copying the one provided.
	 */
	public void set(Matrix22f m) {
		System.arraycopy(m.d, 0, d, 0, d.length);
	}

	/**
	 * Constructs a matrix from the given elements, specified in row-major order.
	 */
	public Matrix22f(float m00, float m01, float m10, float m11) {
		d = new float[] { m00, m01, m10, m11 };
	}

	/**
	 * Sets this matrix from the given elements, specified in row-major order.
	 */
	public void set(float m00, float m01, float m10, float m11) {
		d[0] = m00;
		d[1] = m01;
		d[2] = m10;
		d[3] = m11;
	}

	/**
	 * Returns the given element using row-major ordering.
	 */
	public float get(int i) {
		return d[i];
	}

	/**
	 * Sets the given element using row-major ordering.
	 */
	public void set(int i, float v) {
		d[i] = v;
	}

	/**
	 * Returns the element at the given row and column.
	 */
	public float get(int row, int column) {
		return d[row*2 + column];
	}

	/**
	 * Sets the element at the given row and column.
	 */
	public void set(int row, int column, float v) {
		d[row*2 + column] = v;
	}

	/**
	 * Returns the sum of this matrix and {@code rhs}.
	 */
	public Matrix22f add(Matrix22f rhs) {
		return new Matrix22f(this).addAssign(rhs);
	}

	/**
	 * Adds {@code rhs} to this matrix.
	 */
	public Matrix22f addAssign(Matrix22f rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] += rhs.d[i];
		}
		return this;
	}

	/**
	 * Returns the difference between this matrix and {@code rhs}.
	 */
	public Matrix22f subtract(Matrix22f rhs) {
		return new Matrix22f(this).subtractAssign(rhs);
	}

	/**
	 * Subtracts {@code rhs} from this matrix.
	 */
	public Matrix22f subtractAssign(Matrix22f rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] -= rhs.d[i];
		}
		return this;
	}

	/**
	 * Returns this matrix negated.
	 */
	public Matrix22f negate() {
		return new Matrix22f(this).negateAssign();
	}

	/**
	 * Negates this matrix.
	 */
	public Matrix22f negateAssign() {
		for (int i = 0; i < d.length; ++i) {
			d[i] = -d[i];
		}
		return this;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Vector2f multiply(Vector2f rhs) {
		Vector2f vec = new Vector2f();
		for (int y = 0; y < 2; ++y) {
			float v = 0.0f;
			for (int x = 0; x < 2; ++x) {
				v += get(y, x) * rhs.get(x);
			}
			vec.set(y, v);
		}
		return vec;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Matrix22f multiply(Matrix22f rhs) {
		return new Matrix22f(this).multiplyAssign(rhs);
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Matrix33af multiply(Matrix33af rhs) {
		return new Matrix33af(rhs).preMultiplyAssign(this);
	}

	/**
	 * Multiplies this matrix by {@code rhs}.
	 */
	public Matrix22f multiplyAssign(Matrix22f rhs) {
		float[] dt = new float[d.length];
		for (int y = 0; y < 2; ++y) {
			for (int x = 0; x < 2; ++x) {
				int idx = y*2 + x;
				for (int i = 0; i < 2; ++i) {
					dt[idx] += get(y, i) * rhs.get(i, x);
				}
			}
		}
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Pre-multiplies this matrix by {@code lhs}.
	 */
	public Matrix22f preMultiplyAssign(Matrix22f lhs) {
		float[] dt = new float[d.length];
		for (int y = 0; y < 2; ++y) {
			for (int x = 0; x < 2; ++x) {
				int idx = y*2 + x;
				for (int i = 0; i < 2; ++i) {
					dt[idx] += lhs.get(y, i) * get(i, x);
				}
			}
		}
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Matrix22f multiply(float rhs) {
		return new Matrix22f(this).multiplyAssign(rhs);
	}

	/**
	 * Multiplies this matrix by {@code rhs}.
	 */
	public Matrix22f multiplyAssign(float rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] *= rhs;
		}
		return this;
	}

	/**
	 * Returns the quotient of this matrix and {@code rhs}.
	 */
	public Matrix22f divide(float rhs) {
		return new Matrix22f(this).divideAssign(rhs);
	}

	/**
	 * Divides this matrix by {@code rhs}.
	 */
	public Matrix22f divideAssign(float rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] /= rhs;
		}
		return this;
	}

	/**
	 * Returns whether this matrix is equal to the object {@code o}.
	 */
	@Override
	public boolean equals(Object o) {
		if (o == this) {
			return true;
		} else if (o == null || !(o instanceof Matrix22f)) {
			return false;
		} else {
			return equals((Matrix22f) o);
		}
	}

	/**
	 * Returns a hash code for this object.
	 */
	@Override
	public int hashCode() {
		int bits = 0;
		for (int i = 0; i < d.length; ++i) {
			bits ^= Float.floatToIntBits(d[i]);
		}
		return bits;
	}

	/**
	 * Returns whether this matrix is equal to the matrix {@code rhs}.
	 */
	public boolean equals(Matrix22f rhs) {
		return Arrays.equals(d, rhs.d);
	}

	/**
	 * Returns the transpose of this matrix.
	 */
	public Matrix22f transpose() {
		return new Matrix22f(this).transposeAssign();
	}

	/**
	 * Transposes this matrix.
	 */
	public Matrix22f transposeAssign() {
		float tmp = d[1];
		d[1] = d[2];
		d[2] = tmp;
		return this;
	}

	/**
	 * Returns the inverse of this matrix, or {@code null} if this matrix is not invertible.
	 */
	public Matrix22f invert() {
		return new Matrix22f(this).invertAssign();
	}

	/**
	 * Inverts this matrix, or returns {@code null} if this matrix is not invertible.
	 */
	public Matrix22f invertAssign() {
		float det = determinant();
		if (det == 0.0f) {
			return null;
		}

		float invDet = 1.0f / det;
		float[] dt = new float[] {
				d[3] * invDet,
				-d[1] * invDet,
				-d[2] * invDet,
				d[0] * invDet
		};
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Returns this matrix rotated by {@code a} radians.
	 */
	public Matrix22f rotate(float a) {
		return new Matrix22f(this).rotateAssign(a);
	}

	/**
	 * Rotates this matrix by {@code a} radians.
	 */
	public Matrix22f rotateAssign(float a) {
		float sinA = (float) Math.sin(a);
		float cosA = (float) Math.cos(a);
		Matrix22f rotMat = new Matrix22f(
				cosA, -sinA,
				sinA, cosA);
		return preMultiplyAssign(rotMat);
	}

	/**
	 * Returns this matrix scaled by {@code s}.
	 */
	public Matrix22f scale(Vector2f s) {
		return new Matrix22f(this).scaleAssign(s);
	}

	/**
	 * Scales this matrix by {@code s}.
	 */
	public Matrix22f scaleAssign(Vector2f s) {
		d[0] *= s.get(0);
		d[1] *= s.get(0);
		d[2] *= s.get(1);
		d[3] *= s.get(1);
		return this;
	}

	/**
	 * Returns the determinant of this matrix.
	 */
	public float determinant() {
		return d[0]*d[3] - d[1]*d[2];
	}

	/**
	 * Returns the trace of this matrix.
	 */
	public float trace() {
		return d[0] + d[3];
	}

	/**
	 * Returns the raw underlying mutable array.
	 */
	public float[] getArray() {
		return d;
	}

	/**
	 * Converts this matrix to a 4x4 matrix suitable for graphics transformations.
	 */
	public Matrix44f getGraphicsMatrix() {
		return new Matrix44f(
				d[0], d[1], 0.0f, 0.0f,
				d[2], d[3], 0.0f, 0.0f,
				0.0f, 0.0f, 1.0f, 0.0f,
				0.0f, 0.0f, 0.0f, 1.0f);
	}
}
